/*
 * Created on Feb 12, 2007
 *
 * TODO To change the template for this generated file go to
 * Window - Preferences - Java - Code Style - Code Templates
 */
package org.pricejd.rcx;

import josx.platform.rcx.LCD;
import josx.platform.rcx.Motor;
import josx.platform.rcx.Sensor;
import josx.platform.rcx.SensorConstants;
import josx.platform.rcx.SensorListener;
import josx.platform.rcx.Sound;
import josx.robotics.Navigator;

/**
 * The LightNavigator class contains methods for performing basic navigational
 * movements.  This class uses an encoder wheel attached to a 40 tooth gear
 * to act as the "poor man's rotation sensor".  Alot of this code is adabted from
 * RotationNavigator which comes with Lejos.
 * 
 * TODO: Update the code to use two light sensors.  This way we can have the
 * 		 rotation of each motor measured.  This will ensure the robot moves
 * 		 in a straight line.
 * 
 * @author Jon Price
 * @see josx.robotics.RotationNavigator
 */
public class LightNavigator implements Navigator, SensorConstants{
	// These values may need to be configured.
	private final int whiteValue = 45;
	private final int blackValue = 45;
	private final float COUNTS_PER_CM = .8f;
	private final float COUNTS_PER_DEGREE = .09f;
	
	
	// Motors for steering
	private Motor left;
	private Motor right;
	
	// Rotation Sensor
	private Sensor rotRight;
	private Sensor bumper;
	
	// Moement values
	private int lightCount_dist;
	private int lightCount_angle;
	private boolean countWhite;
	private boolean countBlack;
	
	// Internal states
	private boolean moving;
	private byte command;
	
	// orientation and co-ordinate data
	private float angle;
	private float x;
	private float y;
	
	private float xDest;
	private float yDest;

	private boolean canMove;
	private boolean bRotate;
	
	/*
	 * Allocates a LightNavigator object and initializes it with the
	 * proper motors and sensors.  The x and y values will each equal
	 * 0 (cm's) on initialization, and the starting angle is 0 degrees,
	 * so, if the first move is forward() the robot will run along the
	 * x axix.
	 * 
	 * @param rightMotor The motor used to drive the right wheel e.g. Motor.C.
	 * @param leftMotor The motor used to drive the left wheel e.g. Motor.A.
	 * @param rightRot The light sensor that is counting the rotation on the right.
	 * @param leftRot The light sensor that is counting the rotation on the left.
	 * @param wheelDiameter the diameter of the wheel in centimeters
	 * @param driveLength The distance from the center of the front tire to the
	 * 		center of the back tire.
	 * @param ratio The ratio of sensor rotations to wheel rotations.
	 */
	public LightNavigator(Motor rightMotor, Motor leftMotor, Sensor rightRot, Sensor sBumper) {
		this.right = rightMotor;
		this.left = leftMotor;
		
		this.rotRight = rightRot;
		this.bumper = sBumper;
		this.rotRight.setTypeAndMode(SENSOR_TYPE_LIGHT, SENSOR_MODE_PCT);
		
		moving = false;
		canMove = true;
		
		
		rightRot.addSensorListener(new SensorListener() {
			public void stateChanged(Sensor arg0, int arg1, int arg2) {
				if(arg2 < blackValue && countBlack) {
					if(!bRotate)
						lightCount_dist++;
					else
						lightCount_angle++;
					countBlack = false;
					countWhite = true;
				}
				if(arg2 > whiteValue && countWhite) {
					if(!bRotate)
						lightCount_dist++;
					else
						lightCount_angle++;
					countBlack = true;
					countWhite = false;
				}
			}
		});
		
		sBumper.addSensorListener(new SensorListener() {
			public void stateChanged(Sensor arg0, int arg1, int arg2) {
				stop();
				canMove = false;
				Sound.beepSequence();
				try {
				Thread.sleep(500);
				} catch(Exception e) {
					
				}
			}
		});
	}


	/**
	 * Rotates the the RCX a specific number of degrees in a direction
	 * (+ or -).  This method will return once the rotation is complete.
	 * 
	 * @param angle Angle to rotate in degrees.  A positive calue rotates
	 * 		left, a negative value rotates right.
	 */
	public void rotate(float angle) {
		
		// Calculate the number of intervals of rotation sensor to count
		int count = (int)(COUNTS_PER_DEGREE * angle);
		
		// Turn on light sensor.
		rotRight.activate();
		countBlack = Sensor.S1.readValue() > whiteValue;
		countWhite = Sensor.S1.readValue() < blackValue;
		bRotate = true;
		if(angle > 0) {
			// Start motors.
			right.forward();
			left.backward();
			LCD.showNumber(Math.abs(count));
		} else if (angle < 0) {
			// Start motors.
			right.backward();
			left.forward();
			LCD.showNumber(Math.abs(count));
		}
		moveRCX(Math.abs(count));
		
		// Turn off light sensor and stop motors.
		rotRight.passivate();
		stop();
	}
	
	/**
	 * Moves the RCX for a specified number of counts on the
	 * light/rotation sensor.
	 * 
	 * Expects that the motors are moving.
	 */
	private void moveRCX(int count) {
		moving = true;
		while(moving && canMove) {
			if(lightCount_dist > count || lightCount_angle > count) {
				Motor.A.stop();
				Motor.C.stop();
				moving = false;
			}
		}
	}

	/**
	 * Moves the RCX a specific distance.  A positive value moves it forward
	 * and a negative value moves it backwards.
	 * 
	 * @param dist The positive or negative distnace to move (in cm's).
	 * @throws InterruptedException
	 */
	public void travel(int dist) {
		int count = (int)(dist * COUNTS_PER_CM);
		
		rotRight.activate();
		countBlack = Sensor.S1.readValue() > whiteValue;
		countWhite = Sensor.S1.readValue() < blackValue;
		if(dist > 0) {
			forward();
		} else if (dist < 0) {
			backward();
		}
		
		moveRCX(Math.abs(count));
		rotRight.passivate();
		stop();
	}

	
	public void forward() {
		right.forward();
		left.forward();
	}

	
	public void backward() {
		right.backward();
		left.backward();
	}

	
	public void stop() {
		
		left.stop();
		right.stop();
		
		float centimeters = lightCount_dist / COUNTS_PER_CM;
		
		x = x + (float)(Math.cos(Math.toRadians(angle)) * centimeters);
		y = y + (float)(Math.sin(Math.toRadians(angle)) * centimeters); 

		lightCount_dist = 0;
		lightCount_angle = 0;
	}


	public float getX() {
		return x;
	}


	public float getY() {
		return y;
	}


	public float getAngle() {
		return angle;
	}


	/**
	 * Rotates the RCX robot to point in a certain direction.
	 * @params Angle the angle to rotate to in degrees.
	 */
	public void gotoAngle(float angle) {
		float difference = angle - this.angle;
		while(difference > 180) {
			difference = difference - 360;
		}
		while(difference < -180) {
			difference = difference + 360;
		}
		rotate(difference);
	}



	public void gotoPoint(float x, float y) {
		// Determine the relative points
		float x1 = x - this.x;
		float y1 = y - this.y;
		
		// Calculate angle to go to
		angle = (float)Math.atan2(y1, x1);
		
		// Calculate distance to travel
		float distance;
		if(y1 != 0)
			distance = y1/(float)Math.sin(angle);
		else
			distance = x1/(float)Math.cos(angle);
		
		// Convert angle from rads to degrees
		angle = (float)Math.toDegrees(angle);
		gotoAngle(angle);
		travel(Math.round(distance));
	}

	public void reset() {
		x = 0;
		y = 0;
		angle = 0;
		canMove = true;
	}
	
	public boolean getCanMove() {
		return canMove;
	}
}